#include "mainwindow.h"
#include "ui_mainwindow.h"
#include "FloraLogger.h"

#include <QtConcurrent/QtConcurrent>
#include <QDoubleValidator>

/*

sudo busybox devmem 0x0c303018 w 0xc458
sudo busybox devmem 0x0c303010 w 0xc400
sudo busybox devmem 0x0c303008 w 0xc458
sudo busybox devmem 0x0c303000 w 0xc400
sudo modprobe can
sudo modprobe can_raw
sudo modprobe mttcan
sudo ip link set can0 type can bitrate 500000
sudo ip link set can1 type can bitrate 500000
sudo ip link set up can0
sudo ip link set up can1
cansend can1 1F223344#1122334455667788
candump can0 #设置CAN0为接收状态
 * */

void man_callback_t(uint8_t func_code, uint8_t *src_buff, uint8_t len, void *param)
{
    MainWindow *ptr= (MainWindow *)param;
    offset_t temp_t;
    switch (func_code) {
    case 1:
        if(len == sizeof (offset_t))
        {
            memcpy((void *)&temp_t,src_buff,len);
            ptr->setRegResult(temp_t.x,temp_t.y,temp_t.z,temp_t.yaw,temp_t.pitch,temp_t.roll,temp_t);

        }
        break;
    }
}


MainWindow::MainWindow(QWidget *parent)
    : QMainWindow(parent)
    , ui(new Ui::MainWindow)
{
    ui->setupUi(this);
    //qRegisterMetaType(struct can_frame);
    canSocket = new UdpSocket();
    canSocket->bindPort(50001);
    canT = new CanThread(QString("can0"));
    canT_1 = new CanThread(QString("can1"));
    connect(canSocket,&UdpSocket::dataReceived,this,&MainWindow::dataReceived);
    connect(canT,&CanThread::MCU_Trans,this,&MainWindow::MCU_Trans);
    connect(canT,&CanThread::MCU_Rot,this,&MainWindow::MCU_Rot);
    connect(canT,&CanThread::messageReceived,this,&MainWindow::dealCanMessage,Qt::QueuedConnection);
    connect(canT_1,&CanThread::messageReceived,this,&MainWindow::dealCanMessage,Qt::QueuedConnection);
    ui_init();
    protocol256_parser_init(&pars_t,man_callback_t,(void *)this);

    askMotorPosTimer.setInterval(250);
    autoControlTimer.setInterval(100);
    connect(&askMotorPosTimer,&QTimer::timeout,this,&MainWindow::askMotorPosTask);
    connect(&autoControlTimer,&QTimer::timeout,this,&MainWindow::autoControlTask);
    //askMotorPosTimer.start();
    QWidget *logWid = new QWidget();
    logWid->setGeometry(0,0,500,500);
    FloraLogger::instance().embedInWidget(logWid);
    logWid->show();
    tempWid = logWid;

    ConTrolMode = MODE_MANUAL;
    connect(this,&MainWindow::newKeyInput,this,&MainWindow::reciveKeyCode,Qt::QueuedConnection);
    QtConcurrent::run(QThreadPool::globalInstance(),this,&MainWindow::readKeyTask);
}

void MainWindow::ui_init()
{

    QDoubleValidator * va= new QDoubleValidator(-999.99,999.99,5);
    va->setNotation(QDoubleValidator::StandardNotation);
    ui->le_roll_action_angle->setValidator(va);


    QGridLayout *gridLayout = new QGridLayout(ui->lc_wid);
    LC_A6_Motor* wid = new LC_A6_Motor(4,713614,-713614);
    Motor_id_Map.insert(wid->getId(),wid);
    wid->setSpeed(300);
    wid->setName("横滚A_4");
    wid->setZeroPos(0);
    Motor_name_Map.insert(wid->getName(),wid);
    connect(wid,&LC_A6_Motor::sendMessage,canT,&CanThread::enqueueMessage_1);
    gridLayout->addWidget(wid,0,0);


    wid = new LC_A6_Motor(5,713614,-713614);
    wid->setSpeed(300);
    wid->setZeroPos(0);
    Motor_id_Map.insert(wid->getId(),wid);
    wid->setName("横滚B_5");
    Motor_name_Map.insert(wid->getName(),wid);
    connect(wid,&LC_A6_Motor::sendMessage,canT,&CanThread::enqueueMessage_1);
    gridLayout->addWidget(wid,0,1);

    wid = new LC_A6_Motor(3,25278510,-32531340);
    wid->zeroOffect = (-32531340 + 25278510) / 2;
    wid->setZeroPos(wid->zeroOffect);
    //wid->zeroOffect = 0;
    wid->setSpeed(20000);
    Motor_id_Map.insert(wid->getId(),wid);
    wid->setName("Y位移控制_3");
    Motor_name_Map.insert(wid->getName(),wid);
    connect(wid,&LC_A6_Motor::sendMessage,canT,&CanThread::enqueueMessage_1);
    gridLayout->addWidget(wid,1,0);

    wid = new LC_A6_Motor(6,83016,-383016); // max 83016 min -383016   +-0.8 -> 233016
    wid->zeroOffect = -150000;
    wid->setZeroPos(wid->zeroOffect);
    wid->setSpeed(100);
    Motor_id_Map.insert(wid->getId(),wid);
    wid->setName("偏航_6");
    Motor_name_Map.insert(wid->getName(),wid);
    connect(wid,&LC_A6_Motor::sendMessage,canT,&CanThread::enqueueMessage_1);
    gridLayout->addWidget(wid,0,2);


    wid = new LC_A6_Motor(2,40000000,18090000-100*x_scale); // max 83016 min -383016   +-0.8 -> 233016 40000000  18090000
    wid->zeroOffect = (40000000 + 18090000)/2;
    wid->setZeroPos(wid->zeroOffect);
    wid->setSpeed(20000);
    Motor_id_Map.insert(wid->getId(),wid);
    wid->setName("X偏移_2");
    Motor_name_Map.insert(wid->getName(),wid);
    connect(wid,&LC_A6_Motor::sendMessage,canT,&CanThread::enqueueMessage_1);
    gridLayout->addWidget(wid,1,2);

    auto wid1 = new YeYaGangControl(7,3400,30);
    wid1->setSpeed(1);
    wid1->setName("举升Z轴——7");
    YYG_id_Map.insert(wid1->getId(),wid1);
    connect(wid1,&YeYaGangControl::sendMessage,canT_1,&CanThread::enqueueMessage_1);
    gridLayout->addWidget(wid1,1,1);

    wid1 = new YeYaGangControl(9,1900,100);
    wid1->setSpeed(1);
    wid1->setName("宽抱爪——9");
    YYG_id_Map.insert(wid1->getId(),wid1);
    connect(wid1,&YeYaGangControl::sendMessage,canT_1,&CanThread::enqueueMessage_1);
    gridLayout->addWidget(wid1,0,3);

    wid1 = new YeYaGangControl(8,1600,70,700);
    wid1->setSpeed(1);
    wid1->setName("倾斜俯仰——8");
    YYG_id_Map.insert(wid1->getId(),wid1);
    connect(wid1,&YeYaGangControl::sendMessage,canT_1,&CanThread::enqueueMessage_1);
    gridLayout->addWidget(wid1,2,0);

    wid1 = new YeYaGangControl(10,1780,100);
    wid1->setSpeed(1);
    wid1->setName("窄抱抓——10");
    YYG_id_Map.insert(wid1->getId(),wid1);
    connect(wid1,&YeYaGangControl::sendMessage,canT_1,&CanThread::enqueueMessage_1);
    gridLayout->addWidget(wid1,1,3);
}

MainWindow::~MainWindow()
{
    tempWid->deleteLater();
    KeyThread = false;
    delete ui;
}

void MainWindow::MCU_Trans(float x, float y, float z)
{
    ui->X_show->setNum(x);
    ui->Y_show->setNum(y);
    ui->Z_show->setNum(z);
}

void MainWindow::MCU_Rot(float yaw, float pitch)
{
    ui->yaw_show->setNum(yaw);
    ui->pitch_show->setNum(pitch);
}

void MainWindow::dataReceived(const QHostAddress &address, quint16 port, const QByteArray &data)
{
    protocol_parse_packet(&pars_t,(uint8_t *)data.data(),data.size());
}

void MainWindow::setRegResult(float x, float y, float z, float yaw, float pitch, float roll, offset_t offet)
{
    ui->X_show_2->setNum(x);
    ui->Y_show_2->setNum(y);
    ui->Z_show_2->setNum(z);
    ui->yaw_show_2->setNum(yaw);
    ui->pitch_show_2->setNum(pitch);
    ui->rol_show_2->setNum(roll);
    camera_result = offet;
    f_recive_cameraResult +=1;
}

void MainWindow::sendTest()
{
    static int i=0;
    while(1)
    {
        canT->sendAttitude(i++,i++,i++);
        QThread::msleep(10);
    }

}

void MainWindow::askMotorPosTask()
{
    if(!canT->isRun())return;
    for(QMap<uint8_t ,LC_A6_Motor*>::const_iterator it = Motor_id_Map.constBegin(); it != Motor_id_Map.constEnd();++it)
    {
        it.value()->emitAskPos();
    }
    for(QMap<uint8_t ,YeYaGangControl*>::const_iterator it = YYG_id_Map.constBegin(); it != YYG_id_Map.constEnd();++it)
    {
        it.value()->readCurPos();
    }
}

void MainWindow::autoControlTask()
{
    float error_x = 10.0;
    QString showStr;
    switch (c_controlStrep) {
    case STEP_SET_AXI:
            showStr = "等待识别结果!";
            if(f_recive_cameraResult)
            {
                // 判断要控制那一个轴

                if(fabs( camera_result.x) > error_x )
                {
                    showStr =  QString("X轴差值:%1,进入X轴控制").arg(error_x);
                    autoControlTimer.setInterval(1000);
                    c_controlStrep = STEP_CONTROL_AXI_X;
                    control_x_step = CAL_POS;
                }
            }
            else
            {
                showInfo(showStr);
            }

        break;
    case STEP_CONTROL_AXI_X:
        contralTask_AXI_X();
        break;
    default:
        break;
    }



}

void MainWindow::readKeyTask()
{
    LOG_DEBUG<<"readKeyTask start";
    const char *dev = "/dev/input/event10";
    int fd = open(dev,O_RDONLY);
    if(fd == -1 )
    {
        LOG_DEBUG<<"filae to open dev";
        return;
    }
    struct input_event ev;
    while(KeyThread)
    {
        ssize_t n = read(fd,&ev,sizeof(struct input_event));
        if(n<(ssize_t)sizeof(struct input_event)){

        }
        else
        {
            emit newKeyInput(ev);
        }

    }
}

void MainWindow::contralTask_AXI_X()
{
    float curX,error_X;
    QString showStr;
    int fangxiang ;
    static float targetPos =0;
    error_X = camera_result.x;
    curX = Motor_id_Map[X_MOTOR_ID]->getXY();
    if(fabs(error_X) <10 )
    {
        c_controlStrep = STEP_SET_AXI;
        return ;
    }
    if(f_recive_cameraResult)
    {

        fangxiang = error_X >=0 ? 1:-1;
        targetPos = curX + fangxiang * 0.1;
        Motor_id_Map[X_MOTOR_ID]->setX_Trans(targetPos);

        f_recive_cameraResult =0;
    }
    else{
        //showInfo("等待识别结果!");
    }
    showStr = QString("X差值:%1,X轴当前位置:%2 mm,目标控制位置：%3 mm").arg(error_X).arg(curX).arg(targetPos);
    if(f_recive_cameraResult == 0)
    {
        showStr+="\r等待识别结果!";
    }
    autoControlTimer.setInterval(100);
    showInfo(showStr);
}

void MainWindow::showInfo(QString str)
{
    ui->lbcontol->setText(str);
}

void MainWindow::closeEvent(QCloseEvent *event)
{
    tempWid->deleteLater();

}

void MainWindow::keyPressEvent(QKeyEvent *event)
{
    if(event->key()==Qt::Key_Up)
    {

    }
}


void MainWindow::on_setBtn_clicked()
{
    canT->sendAttitude(ui->yaw_set->text().toFloat(),ui->pitch_set->text().toFloat(),ui->roll_set->text().toFloat());
    canT->sendOffset(ui->x_lineedit->text().toFloat(),ui->y_Set->text().toFloat(),ui->z_set->text().toFloat(),ui->CamHkAckBox->isChecked());
}

void MainWindow::on_setBtn_2_clicked()
{
    LOG_DEBUG<<"开始线程";
    canT->start();
    canT_1->start();
    //QtConcurrent::run(this,&MainWindow::sendTest);
}

void MainWindow::dealCanMessage(const can_frame frame)
{
    if(frame.can_id<0x700)
    {
        //LOG_DEBUG<<"接收ID：0x "<<QString::number(frame.can_id,16)<<"数据："<<QByteArray((char *)frame.data,8).toHex(' ');
    }

    uint8_t id= frame.can_id &0x7f;
    if(Motor_id_Map.contains(id))
    {
        Motor_id_Map[id]->reciveMessage(frame);
    }
    if(YYG_id_Map.contains(id))
    {
        YYG_id_Map[id]->reciveMessage(frame);
    }
}

void MainWindow::reciveKeyCode(input_event ev)
{
    switch(ev.type)
    {
     case EV_KEY:

        LOG_DEBUG<<"按键按下"<<ev.code<<ev.value;
        break;
     case EV_ABS:
        LOG_DEBUG<<"油门:"<<ev.code<<ev.value;
        break;
    }
}

void MainWindow::on_cb_askPos_clicked(bool checked)
{
    if(checked)
    {
        askMotorPosTimer.start();
    }else
    {
        askMotorPosTimer.stop();
    }
}

void MainWindow::on_bn_roll_shun_action_clicked()
{
    float angle = ui->le_roll_action_angle->text().toFloat();
    if(Motor_id_Map.contains(ROLL_MOTOR_A_ID) && Motor_id_Map.contains(ROLL_MOTOR_B_ID))
    {
        Motor_id_Map[ROLL_MOTOR_A_ID]->baseCurPoseAdd(angle);
        Motor_id_Map[ROLL_MOTOR_B_ID]->baseCurPoseDown(angle);
    }
}



void MainWindow::on_setBtn_4_clicked()
{
    on_setBtn_2_clicked();
}

void MainWindow::on_btnEnableMotor_clicked()
{
    for(QMap<uint8_t ,LC_A6_Motor*>::const_iterator it = Motor_id_Map.constBegin(); it != Motor_id_Map.constEnd();++it)
    {
        it.value()->enabelMotor();
    }
}

void MainWindow::on_bn_yaw_action_clicked()
{
    Motor_id_Map[YAW_MOTOR_ID]->setYawAngle(ui->le_yaw_action_angle->text().toFloat());
}

void MainWindow::on_bn_Ypianyi_action_clicked()
{
    Motor_id_Map[Y_MOTOR_ID]->setY_Trans(ui->le_Ypianyi->text().toFloat());
}

void MainWindow::on_bn_Xpianyi_action_clicked()
{
    Motor_id_Map[X_MOTOR_ID]->setX_Trans(ui->le_Xpianyi->text().toFloat());
}

void MainWindow::on_cb_controlMode_clicked(bool checked)
{
    if(checked)
    {
        ConTrolMode = MODE_AUTO;
        autoControlTimer.start(1000);
        showInfo("开始自动任务");
    }
    else
    {
        ConTrolMode = MODE_MANUAL;
        autoControlTimer.stop();
    }
}

void MainWindow::on_btn_moNiSet_clicked()
{
    offset_t temp ;
    temp.x = ui->le_moni_x->text().toFloat();
    temp.y = ui->le_moni_y->text().toFloat();
    temp.z = ui->le_moni_z->text().toFloat();
    temp.roll = ui->le_moni_roll->text().toFloat();
    temp.yaw = ui->le_moni_yaw->text().toFloat();
    temp.pitch = ui->le_moni_pitch->text().toFloat();
    setRegResult(temp.x,temp.y,temp.z,temp.yaw,temp.pitch,temp.roll,temp);
}
